#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S3, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     DriveFL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     DriveBL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     LiftL,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     DriveFR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     DriveBR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_1,     LiftR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     Roller,        tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S3_C2_1,    TubeG,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    LDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    RDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_4,    SensorServo,          tServoStandard)
#pragma config(Servo,  srvo_S3_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

bool OverrideBtnPress;

bool TubeGUp;

int powerVal[128]= {
	0,
	0,
	0,
	0,
	0,
	0,
	0,
	0,
	0,
	0,
	0,
	1,
	1,
	1,
	1,
	1,
	2,
	2,
	2,
	2,
	3,
	3,
	3,
	3,
	4,
	4,
	4,
	5,
	5,
	5,
	6,
	6,
	7,
	7,
	8,
	8,
	8,
	9,
	9,
	10,
	10,
	11,
	12,
	12,
	13,
	13,
	14,
	14,
	15,
	16,
	16,
	17,
	18,
	18,
	19,
	20,
	20,
	21,
	22,
	23,
	23,
	24,
	25,
	26,
	27,
	27,
	28,
	29,
	30,
	31,
	32,
	33,
	33,
	34,
	35,
	36,
	37,
	38,
	39,
	40,
	41,
	42,
	43,
	44,
	45,
	46,
	48,
	49,
	50,
	51,
	52,
	53,
	54,
	55,
	57,
	58,
	59,
	60,
	62,
	63,
	64,
	65,
	67,
	68,
	69,
	70,
	72,
	73,
	75,
	76,
	77,
	79,
	80,
	81,
	83,
	84,
	86,
	87,
	89,
	90,
	92,
	93,
	95,
	96,
	98,
	100,
	100,
	100
};
task DriveL() {
	int powerDrive, abspowerDrive;
	for(;;) {
		if (joy1Btn(9) == 0)

		{

			//The following code is for the Drivetrain.  It has been coded that all drive motion is controlled by Joystick 1.

			//Left Drive
			if(nMotorEncoder[LiftR] < 850 || TubeGUp == true)
			{
				abspowerDrive=abs(joystick.joy1_y1);
				if (abspowerDrive > 127 ) {
					abspowerDrive=127;
				}
				powerDrive=powerVal[abspowerDrive];
				if( joystick.joy1_y1 >= 0)
				{
					motor[DriveFL] = powerDrive;
					motor[DriveBL] = powerDrive;
				}

				else
				{

					motor[DriveFL] = -powerDrive;
					motor[DriveBL] = -powerDrive;

				}
			}

			else

			{

				motor[DriveFR] = 0;
				motor[DriveFL] = 0;
				motor[DriveBR] = 0;
				motor[DriveBL] = 0;


			}
		}
	}
}
//Right Drive
task DriveR() {

	int powerDrive,abspowerDrive;
	for(;;) {
		if(nMotorEncoder[LiftR] < 850 || TubeGUp == true) {
			abspowerDrive=abs(joystick.joy1_y2);
			if (abspowerDrive > 127 ) { abspowerDrive=127; }
			powerDrive=powerVal[abspowerDrive];
			if( joystick.joy1_y2 >= 0)
			{

				motor[DriveFR] = powerDrive;
				motor[DriveBR] = powerDrive;

			}

			else
			{

				motor[DriveFR] = -powerDrive;
				motor[DriveBR] = -powerDrive;

			}
		}
		/*
		if (joystick.joy1_y2 >= 0 )
		{
		powerDrive=powerVal[joystick.joy1_y2];
		}
		else {
		powerDrive=-1*powerVal[joystick.joy1_y2];
		}
		motor[DriveFR] = powerDrive;
		motor[DriveBR] = powerDrive;
		*/
		/*
		if(joystick.joy1_y2 > 10 || joystick.joy1_y2 < -10)
		{

		motor[DriveFR] = joystick.joy1_y2;
		motor[DriveBR] = joystick.joy1_y2;

		}
		*/
		else
		{

			motor[DriveFR] = 0;
			motor[DriveBR] = 0;

		}

	}
}


task DriveS()
{

	OverrideBtnPress = true;

	StopTask (DriveR);
	StopTask (DriveL);

	wait1Msec(100);


	while (joy2Btn(9) == 1 & joy2Btn(10) == 1)
	{
		int val;
		int val2;

		if(joystick.joy1_y1 > 10 | joystick.joy1_y1 < -10)
		{
			val = joystick.joy1_y1/10;
			motor[DriveFL] = val;
			motor[DriveBL] = val;
		}
		else
		{

			motor[DriveFL] = 0;
			motor[DriveBL] = 0;

		}



		if(joystick.joy1_y2 > 10 || joystick.joy1_y2 < -10)
		{
			val2 = joystick.joy1_y2/10;
			motor[DriveFR] = val2;
			motor[DriveBR] = val2;
		}
		else
		{

			motor[DriveFR] = 0;
			motor[DriveBR] = 0;

		}

	}

	motor[DriveFR] = 0;
	motor[DriveBR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBL] = 0;

	StartTask (DriveR);
	StartTask (DriveL);

	wait1Msec(100);

	OverrideBtnPress = false;

	StopTask(DriveS);


}
void LooptoBreakallLoops() { /** *sam* */
	if(joy2Btn(11) == 1 & joy2Btn(12) == 1) {
		motor[LiftL] = 0;
		motor[LiftR] = 0;
		wait1Msec(2000);
		return;
	}
}
task main()
{
	waitForStart();

	//Defining all necessary Variables.

	bool LiftUp;
	bool DoorBtnPress;
	bool EmergencyBtnPress;


	//Setting all variables to their necessary values.

	TubeGUp = false;
	LiftUp = false;
	DoorBtnPress = false;
	EmergencyBtnPress = false;
	OverrideBtnPress = false;

	nMotorEncoder[Roller] = 0;
	nMotorEncoder[LiftR] = 0;
	nMotorEncoder[LiftL] = 0;

	wait1Msec(100);

	servo[LDoor] = 0;
	servo[RDoor] = 256;
	servo[TubeG] = 186;
	servo[SensorServo] = 253;

	StopTask(DriveS);

	wait1Msec(500);

	StartTask(DriveL);
	StartTask(DriveR);


	while(true)

	{

		getJoystickSettings(joystick);

		if(joy2Btn(9) == 1 && joy2Btn(10) == 1 && OverrideBtnPress == false)
		{

			StartTask (DriveS);

			OverrideBtnPress = true;

		}

		//The following code is for the roller.  It has been coded so that the Roller is controlled by Joystick 2.

		//Normal Operation

		if(LiftUp == false && TubeGUp == false)
		{

			if(joy2Btn(8) == 1 && joy2Btn(7) == 0)

			{

				motor[Roller] = 100;

			}



			//Reverse Operation.  This is in case we accidentally pick up more than five balls.

			if(joy2Btn(7) == 1 && joy2Btn(8) == 0)

			{

				motor[Roller] = -100;

			}

			//If both triggers are pressed, stop the motor.


			if(joy2Btn(5) == 1)
			{

				motor[Roller] = 0;

			}

		}

		//If the lift is up for any reason, ball dispensing or tube grabbing, the roller MUST stay off.

		else
		{

			motor[Roller] = 0;

		}

		//The following code is for the tube-grabber.  It has been coded so that the tube-grabber is controlled by Joystick 1.

		if(joy1Btn(4) == 1)
		{

			TubeGUp = true;

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;


			while(nMotorEncoder[LiftR] < 250)
			{

				motor[LiftR] = 100;
				motor[LiftL] = 100;

			}

			motor[LiftR] = 15;
			motor[LiftL] = 15;

			wait1Msec(350);

			servo[TubeG] = 250;


		}

		if(joy1Btn(3) == 1)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;


			servo[TubeG] = 186;

			wait1Msec(350);
		}


		if (joy1Btn(2) == 1)

		{


			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;

			servo[TubeG] = 186;

			while (nMotorEncoder[LiftR] > 400)

			{

				motor[Roller] = 0;

				motor[LiftR] = 1;

				motor[LiftL] = 1;

			}

			while(nMotorEncoder[LiftR] >= 50)

			{

				motor[Roller] = 0;

				motor[LiftR] = -1;
				motor[LiftL] = -1;

			}

			motor[LiftR] = -1;

			motor[LiftL] = -1;

			wait1Msec(1000);

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(500);

			motor[Roller] = 0;

			motor[LiftR] = 0;
			motor[LiftL] = 0;

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;


			LiftUp = false;

			TubeGUp = false;


		}




		//		//The following code is to control the lift.


		if(joy2Btn(1) == 1)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(250);

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;

			wait1Msec(100);

			while(nMotorEncoder[LiftR] < 900)
			{
				motor[LiftR] = 100;
				motor[LiftL] = 100;
				LooptoBreakallLoops();
				/* *sam* */
			}

			motor[LiftR] = 15;
			motor[LiftL] = 15;

			LiftUp = true;

		}



		if(joy2Btn(2) == 1)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(250);

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;

			wait1Msec(100);

			while(nMotorEncoder[LiftR] < 1650)
			{

				motor[LiftR] = 100;
				motor[LiftL] = 100;
				LooptoBreakallLoops();
				/* *Sam* */
			}

			motor[LiftR] = 15;
			motor[LiftL] = 15;

			LiftUp = true;

		}



		if(joy2Btn(3) == 1)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(250);

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;

			wait1Msec(100);

			while(nMotorEncoder[LiftR] < 2400)
			{

				motor[LiftR] = 100;
				motor[LiftL] = 100;
				LooptoBreakallLoops();
				/* *Sam* */
			}

			motor[LiftR] = 15;
			motor[LiftL] = 15;

			LiftUp = true;
		}
		if(joy2Btn(4) == 1)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(250);

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;

			wait1Msec(100);

			while(nMotorEncoder[LiftR] < 3240)
			{

				motor[LiftR] = 100;
				motor[LiftL] = 100;
				LooptoBreakallLoops();
				/* *Sam* */
			}

			motor[LiftR] = 15;
			motor[LiftL] = 15;


			motor[LiftR] = 100;
			motor[LiftL] = 100;

			wait1Msec(1000);

			motor[LiftR] = 25;
			motor[LiftL] = 25;

			LiftUp = true;

		}


		if(joy2Btn(11) == 1)

		{

			if(joy2Btn(1) == 1)
			{

				motor[Roller] = 0;

				motor[DriveFR] = 0;
				motor[DriveFL] = 0;
				motor[DriveBR] = 0;
				motor[DriveBL] = 0;


				while(nMotorEncoder[LiftR] > 900)
				{

					motor[LiftR] = 1;
					motor[LiftL] = 1;

				}

				while(nMotorEncoder[LiftR] < 900)
				{

					motor[LiftR] = 75;
					motor[LiftL] = 75;

				}

				motor[LiftR] = 15;
				motor[LiftL] = 15;

				LiftUp = true;

			}



			if(joy2Btn(2) == 1)
			{

				motor[Roller] = 0;

				motor[DriveFR] = 0;
				motor[DriveFL] = 0;
				motor[DriveBR] = 0;
				motor[DriveBL] = 0;


				while(nMotorEncoder[LiftR] > 1650)
				{

					motor[LiftR] = 1;
					motor[LiftL] = 1;

				}

				while(nMotorEncoder[LiftR] < 1650)
				{

					motor[LiftR] = 75;
					motor[LiftL] = 75;

				}

				motor[LiftR] = 15;
				motor[LiftL] = 15;

				LiftUp = true;

			}



			if(joy2Btn(3) == 1)
			{

				motor[Roller] = 0;

				motor[DriveFR] = 0;
				motor[DriveFL] = 0;
				motor[DriveBR] = 0;
				motor[DriveBL] = 0;


				while(nMotorEncoder[LiftR] > 2400)
				{

					motor[LiftR] = 1;
					motor[LiftL] = 1;

				}

				while(nMotorEncoder[LiftR] < 2400)
				{

					motor[LiftR] = 75;
					motor[LiftL] = 75;

				}

				motor[LiftR] = 15;
				motor[LiftL] = 15;

				LiftUp = true;

			}

		}

		//Zeroes the lift.
		if(joy2Btn(6) == 1 && TubeGUp == false)
		{

			motor[Roller] = 0;

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;


			while (nMotorEncoder[LiftR] > 800)

			{

				motor[Roller] = 0;

				motor[LiftR] = 1;

				motor[LiftL] = 1;

			}

			while(nMotorEncoder[LiftR] >= 50)

			{

				motor[Roller] = 0;

				motor[LiftR] = -1;
				motor[LiftL] = -1;

			}

			motor[LiftR] = -1;

			motor[LiftL] = -1;

			wait1Msec(1000);

			motor[LiftR] = 5;
			motor[LiftL] = 5;

			wait1Msec(750);

			motor[Roller] = 0;

			motor[LiftR] = 0;
			motor[LiftL] = 0;

			nMotorEncoder[LiftR] = 0;
			nMotorEncoder[LiftL] = 0;

			wait1Msec(100);

			LiftUp = false;

		}



		//The following code is to control the doors.

		if(LiftUp == true && TubeGUp == false)
		{

			if(joy1Btn(8) == 1)
			{

				DoorBtnPress = true;

			}

			while(DoorBtnPress == true)
			{

				wait1Msec(50);

				servo[RDoor] = 172;
				wait1Msec(2000);

				servo[LDoor] = 65;
				wait1Msec(1000);

				servo[RDoor] = 256;
				servo[LDoor] = 0;

				wait1Msec(500);
				DoorBtnPress = false;

			}




		}

		if(joy1Btn(5) == 1 && joy1Btn(6) == 1)

		{

			nMotorEncoder[LiftR] = 0;

			nMotorEncoder[LiftL] = 0;

		}

	}

}
